% 定义各个连杆和关节类型，默认为转动关节（可选为平移关节）
%       theta    d       a   alpha
L1=Link([   0    0       0   pi/2], 'standard'); L1.qlim = [-pi/2, pi/2];
L2=Link([   0    0   0.045      0], 'standard'); L2.qlim = [-pi/2, pi/2];
L3=Link([   0    0   0.045      0], 'standard'); L3.qlim = [-pi/2, pi/2]; L2.offset = pi/2;
L4=Link([   0    0   0.045   pi/2], 'standard'); L4.qlim = [-pi/2, pi/2];
L5=Link([   0    0       0      0], 'standard'); L5.qlim = [-pi/2, pi/2];

robot=SerialLink([L1, L2, L3, L4, L5]); % 将5个关节组成机械臂
robot.name='5-DOF Robotic Arm';
robot.display(); % 在命令行界面中展示机械臂的DH参数表
view(3); % 解决robot.teach()和plot的索引超出报错
robot.teach();
% robot.plot([0 pi/2 0 0 0]); % 使用运动学模型，配置关节转角theta，展现机械臂的位姿

joint(:,1) = linspace(-pi/2, pi/2);
joint(:,2) = linspace(-pi/2, pi/2);
joint(:,3) = linspace(-pi/2, pi/2);
joint(:,4) = linspace(-pi/2, pi/2);
joint(:,5) = linspace(-pi/2, pi/2);
robot.plot(joint, 'jointdiam', '1', 'fps', 100, 'trail', 'r-')
